/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

/*
*/

#include <iostream>
#include <math.h>
#include "gimi.h"
#include "Map/MapTypes.hpp"
#include "Map/MapData.hpp"
#include "owndebug.h"
#include "binbag.h"

#define BLOCK_WIDTH 1.0
#define RESOLUTION 0.1
#define DATA_SIZE int((BLOCK_WIDTH/RESOLUTION+1)*(BLOCK_WIDTH/RESOLUTION+1))

std::string targetName;

void initGIMI(gimi::GIMI *gim)
{
	
	int r;
	r=gim->connectToHubEx("asrobo.hut.fi",50002,"ground_model_update_tester");
	if (r==GIMI_OK)
	{
		printf("Connected\n");
		
		gim->addProvidedService(GIMI_PROTOCOL_GRID,GIMI_IGNOREMINORTYPE,"ground_model");
		
	} else if (r==GIMI_INVALIDNAME){
		//wxString s=_T("Couldn't connect GIMI: Name ");
		//s << name << "already in use.";
		//wxMessageBox(s);
		printf("Couldn't connect GIMI: Name already in use.\n");
	} else {
		//wxMessageBox("Couldn't connect GIMI: GIMI_ERROR.");
		printf("Couldn't connect GIMI: GIMI_ERROR.\n");
	}

  
}

void sendCmd(gimi::GIMI *gim, MaCI::Map::CMapData *mapData)
{
	
	printf("sending data,\n");
	mapData->Print(1);

	int r=gim->send(targetName,mapData->GetBinBagContainerPtr()->GetBuffer(),
			mapData->GetBinBagContainerPtr()->GetBufferSize(),GIMI_PROTOCOL_GRID,GIMI_IGNOREMINORTYPE);
	
	if (r==GIMI_OK)
	{
		printf("sent data\n");
	} else if (r==GIMI_ERROR) {
		printf("couldnt send, GIMI_ERROR\n");
	} else if (r==GIMI_NOCONNECTION) {
		printf("couldnt send, GIMI_NOCONNECTION\n");
	}
}

int main(void)
{
	gimi::GIMI gim;
	gim::binbag::CBinBag binbag;
	MaCI::Map::CMapData mapData;

	mapData.EncodeTo(&binbag);

	MaCI::Map::TMapHeader header;
  
	float *hmdata;//[DATA_SIZE];
	header.resolution=RESOLUTION;
	
  
	bool go=true;

	debugInit();
	debugSetGlobalDebugLvl(1);
  debugAddMode(DEBUG_MODE_IMMEDIATE_FLUSH);

	initGIMI(&gim);

	char s[100];
	printf("Give target terrain name\n");
	scanf("%s",s);
	targetName=s;

	while (go)
	{
		float val;
		int samples;
    MaCI::Map::TMapLine line;
    MaCI::Map::THeightMapBlockHeader blockheader;

    dPrint(1,"header res=%f", header.resolution);

    //Remember to set map header first!
    if (mapData.SetMapHeader(header))
      printf("Succesfully set header\n");
    else
      printf("Failed to set header\n");

    mapData.SetTimestamp(MaCI::Common::TTimestamp(1,1));

		printf("give height map position x, y , size w, h and ground value in format x y w h v\n");
		scanf("%f %f %f %f %f",&(blockheader.x),&(blockheader.y),&(blockheader.width), &(blockheader.height),&val);
		samples=lrint((blockheader.width/RESOLUTION+1.0)*(blockheader.height/RESOLUTION+1.0));
		blockheader.dataSize=samples*sizeof(float);

		hmdata=new float[samples];
		printf("samples=%d\n",samples);
		for (int i=0;i<samples;i++)
			hmdata[i]=val;

    printf("give wall endpoints in format x1 y1 x2 y2\n");
    scanf("%f %f %f %f",&line.x1,&line.y1,&line.x2,&line.y2);

    if (mapData.AddHeightMapBlock(blockheader,(char*)hmdata))
      printf("Succesfully added height map block\n");
    else
      printf("Failed to add height map block\n");

    if (mapData.AddMapLine(line))
      printf("Succesfully added line\n");
    else
      printf("Failed to add line\n");

    mapData.Print(1);
  
		sendCmd(&gim,&mapData);

		mapData.Reset();

		delete [] hmdata;

	}

}
